#include "mult_lidar/voxel_voting_filter.hpp"
#include <cmath>
#include <limits>

namespace mult_lidar{

    VoxelVotingFilter::VoxelVotingFilter(float resolution, int init_score, int min_score)
        : resolution_(resolution), init_score_(init_score), min_score_(min_score) {}

    
    void VoxelVotingFilter::setParameters(float resolution, int init_score, int min_score){

        resolution_ = resolution;
        init_score_ = init_score;
        min_score_ = min_score;

    }

    void VoxelVotingFilter::processFrame(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud){

        //临时存储当前帧激活的体素
        std::unordered_set<VoxelKey, VoxelHash> current_frame_voxels;

        //1、处理有返回值的点云空间
        for(const auto&point : *cloud){
            VoxelKey key = pointToVoxel(point);
            current_frame_voxels.insert(key);

            //更新有值点云打分
            auto &score = voxel_scores_[key];
            score = score > 0 ? score + 1 : init_score_ + 1;
        }

        //2、处理无返回值的点云空间
        Eigen::Vector4f sensor_origin(0, 0, 0, 1);

        for(const auto& point : *cloud) {
            //计算传感器到该点云的射线
            std::vector<VoxelKey> ray_voxels = computeRayVoxels(sensor_origin, point);

            for(const auto& key : ray_voxels){
                //跳过终点，处理为占用
                if(current_frame_voxels.find(key) != current_frame_voxels.end())
                continue;

                //更新无值点云分数
                auto& score = voxel_scores_[key];
                score = score > 0 ? score -1 : init_score_ - 1;
            }
        }


        //分数过滤
        pcl::PointCloud<pcl::PointXYZI>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZI>);
        for(const auto& point : *cloud){
            VoxelKey key = pointToVoxel(point);
            auto it = voxel_scores_.find(key);
            //只保留阈值之内的点云
            if(it != voxel_scores_.end() && it->second >= min_score_){
                filtered->push_back(point);
            }
        }

        cloud->swap(*filtered);
    }

    VoxelVotingFilter::VoxelKey VoxelVotingFilter::pointToVoxel(const pcl::PointXYZI& point) const{
        return{
            static_cast<int>(std::floor(point.x / resolution_)),
            static_cast<int>(std::floor(point.y / resolution_)),
            static_cast<int>(std::floor(point.z / resolution_))
        };
    }

    std::vector<VoxelVotingFilter::VoxelKey> VoxelVotingFilter::computeRayVoxels(
        const Eigen::Vector4f& origin, const pcl::PointXYZI& end) const{
            std::vector<VoxelKey> voxels;
            //减少采样点提升性能
            const int steps = 20;
            for(int i = 0; i < steps; ++i){
                float t = static_cast<float>(i) / steps;
                Eigen::Vector4f pt = origin + t * (Eigen::Vector4f(end.x, end.y, end.z, 1) - origin);

                pcl::PointXYZI temp;
                temp.x = pt.x();
                temp.y = pt.y();
                temp.z = pt.z();
                voxels.push_back(pointToVoxel(temp));
            }

            return voxels;
        }
        
    
}